﻿using Microsoft.Kinect;
using System;

using Kinect_COM;

namespace Kinect_NameSpace
{

    /*
     * Enum For Retrieving Data and Frame Width and Height 
     */
    public enum StreamType
    {
        Stream_Type_Color,
        Stream_Type_Depth,
    }
     

    public class KinectController
    {
        
        private byte[] m_ColorDataBuffer = null;
        private byte[] m_DepthDataBuffer = null;

        private Skeleton[] m_SkeletonsBuffer = null;

        private KinectSensor m_NuiSensor = null;

        /*
         * Constructor: Retrieves the Kinect Sensor
         */
        public KinectController()
        {
            if (KinectSensor.KinectSensors.Count == 0)
            {
                Console.WriteLine("No Kinect!");
            }
            else
            {
                //Work With The first sensor
                m_NuiSensor = KinectSensor.KinectSensors[0];
            }

        }

        /*
         * Destructor: Destroys the Kinect Sensor instance
         */
        ~KinectController()
        {
            if (m_NuiSensor != null)
                m_NuiSensor.Dispose();
        }

        /*
         * Starts the sensor and enables streams accordingly
         * Returns true if success, false otherwise
         */
        public bool StartKinect(byte bStreamFlags, int nColorStreamSize, int nDepthStreamSize)
        {
            if (Start())
            {
                if ((bStreamFlags & StreamTypeConstants.BITMASK_DEPTH) > 0)
                {
                    switch (nDepthStreamSize)
                    {
                        case DepthStreamConstants.DEPTH_STREAM_320_240_30:
                            initDepthStream(DepthImageFormat.Resolution320x240Fps30);
                            break;
                        case DepthStreamConstants.DEPTH_STREAM_640_480_30:
                            initDepthStream(DepthImageFormat.Resolution320x240Fps30);
                            break;
                        case DepthStreamConstants.DEPTH_STREAM_80_60_30:
                            initDepthStream(DepthImageFormat.Resolution80x60Fps30);
                            break;
                        default:
                            initDepthStream(DepthImageFormat.Resolution640x480Fps30);
                            break;
                    }
                }

                if ((bStreamFlags & StreamTypeConstants.BITMASK_COLOR) > 0)
                {
                    switch (nColorStreamSize)
                    {
                        case ColorStreamConstants.COLOR_STREAM_RGB_640_480_30:
                            initColorStream(ColorImageFormat.RgbResolution640x480Fps30);
                            break;
                        case ColorStreamConstants.COLOR_STREAM_RGB_1280_960_12:
                            initColorStream(ColorImageFormat.RgbResolution1280x960Fps12);
                            break;
                        case ColorStreamConstants.COLOR_STREAM_RGB_640_480_15:
                            initColorStream(ColorImageFormat.RgbResolution640x480Fps30);
                            break;
                        case ColorStreamConstants.COLOR_STREAM_IR_640_480_30:
                            initColorStream(ColorImageFormat.InfraredResolution640x480Fps30);
                            break;
                        default:
                            initColorStream(ColorImageFormat.RgbResolution640x480Fps30);
                            break;

                            
                    }
                }

                if ((bStreamFlags & StreamTypeConstants.BITMASK_SKELETON) > 0)
                    initSkeletonStream();

                return true;
            }
            else
                return false;
        }

        /*
         * Stops the Kinect Sensor
         */
        public void StopKinect()
        {
            if (m_NuiSensor != null)
                m_NuiSensor.Stop();
        }

        /*
         * Sets Elevation Angle
         */
        public void setElevationAngle(int nAngle)
        {
            if (m_NuiSensor == null)
                return;
            if (nAngle < m_NuiSensor.MaxElevationAngle && nAngle > m_NuiSensor.MinElevationAngle)
                m_NuiSensor.ElevationAngle = nAngle;
        }

        /*
         * Retrieves Current Elevation Angle
         */
        public int getElevationAngle()
        {
            if (m_NuiSensor == null)
                return -1;
            return m_NuiSensor.ElevationAngle;
        }

        /*
         * Starts Kinect Sensor
         * Return true if success, false otherwise
         */
        private bool Start()
        {
            if (m_NuiSensor == null)
            {
                Console.WriteLine("Not initialized");
                return false;
            }
            else
            {
                m_NuiSensor.Start();
                return true;
            }
        }

        /*
         * Fires When a Color Frame is Ready
         * Copies Color Data in buffer
         */
        private void nui_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            ColorImageFrame cFrame = e.OpenColorImageFrame();
            if (cFrame == null)
                return;

            cFrame.CopyPixelDataTo(m_ColorDataBuffer);
            cFrame.Dispose();
        }

        /*
         * Fires when a Depth Frame is Ready
         * Copies Frame Data in buffer
         */
        private void nui_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            DepthImageFrame dFrame = e.OpenDepthImageFrame();
            if (dFrame == null)
                return;

            short[] depthData = new short[m_NuiSensor.DepthStream.FramePixelDataLength];
            dFrame.CopyPixelDataTo(depthData);
            ConvertDepthDatatoBytes(depthData);
            dFrame.Dispose();
        }

        public byte[] getData(StreamType type)
        {
            switch (type)
            {
                case StreamType.Stream_Type_Color:
                    return m_ColorDataBuffer;
                case StreamType.Stream_Type_Depth:
                    return m_DepthDataBuffer;
                default:
                    return null;
            }

        }

        public int getStreamWidth(StreamType type)
        {
            if (m_NuiSensor == null)
                return -1;

            switch (type)
            {
                case StreamType.Stream_Type_Color:
                    return m_NuiSensor.ColorStream.FrameWidth;

                case StreamType.Stream_Type_Depth:
                    return m_NuiSensor.DepthStream.FrameWidth;

            }

            return -1;

        }

        public int getStreamHeight(StreamType type)
        {
            if (m_NuiSensor == null)
                return -1;

            switch (type)
            {
                case StreamType.Stream_Type_Color:
                    return m_NuiSensor.ColorStream.FrameHeight;

                case StreamType.Stream_Type_Depth:
                    return m_NuiSensor.DepthStream.FrameHeight;

            }

            return -1;

        }

        private void initColorStream(ColorImageFormat format)
        {
            m_NuiSensor.ColorStream.Enable(format);
            m_ColorDataBuffer = new byte[m_NuiSensor.ColorStream.FramePixelDataLength];
            m_NuiSensor.ColorFrameReady += nui_ColorFrameReady;
 
        }

        private void initDepthStream(DepthImageFormat format)
        {
            m_NuiSensor.DepthStream.Enable(format);
            m_DepthDataBuffer = new byte[m_NuiSensor.DepthStream.FramePixelDataLength];
            m_NuiSensor.DepthFrameReady += nui_DepthFrameReady;
        }

        private void initSkeletonStream()
        {
        }

        private void ConvertDepthDatatoBytes(short[] depthData)
        {
            if (depthData == null || m_DepthDataBuffer == null)
                return;
            int nDepthSourceIndex = 0;

            for (int i = 0; i < depthData.Length; ++i)
            {
                // discard the portion of the depth that contains only the player index
                short depth = (short)(depthData[i] >> DepthImageFrame.PlayerIndexBitmaskWidth);

                // to convert to a byte we're looking at only the lower 8 bits
                // by discarding the most significant rather than least significant data
                // we're preserving detail, although the intensity will "wrap"
                // add 1 so that too far/unknown is mapped to black
                byte intensity = (byte)((depth + 1) & byte.MaxValue);
                m_DepthDataBuffer[nDepthSourceIndex++] = intensity;

            }
        }
    }
}